
// Copyright 2009 David Lee Miller
//
// This file is part of POSSM (Python Open Source Scanning Microscopy).
//
// POSSM is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// POSSM is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License         
// along with POSSM.  If not, see <http://www.gnu.org/licenses/>.
//                                                                           
/////////////////////////////////////////////////////////////////////////

#include "Python.h"
#include "comedilib.h"
#include <math.h>
#include <stdio.h>
#include <time.h>
#include <sys/time.h>

int aref = AREF_GROUND; /* more on this later */
comedi_t *it0;
comedi_t *it1;
int devno0 = -1;
int devno1 = -1;

static PyObject *py_initDevs(PyObject *self, PyObject *args)
{
    char device0[20],device1[20];
    if (!PyArg_ParseTuple(args, "ii",&devno0,&devno1))
        return NULL;
    if (devno0!=-1) sprintf(device0,"/dev/comedi%d",devno0);
    if (devno1!=-1) sprintf(device1,"/dev/comedi%d",devno1);
    it0=comedi_open(device0);
    it1=comedi_open(device1);
    return Py_None;
}

static PyObject *py_microsleep(PyObject *self, PyObject *args)
{
    int useconds;
    if (!PyArg_ParseTuple(args, "i",&useconds))
        return NULL;
    if (useconds>15){
        struct timespec tim1;
        int nanoperiod = (useconds-15)*1000; //to compensate for a ~1us call time into python
        clock_gettime(CLOCK_REALTIME, &tim1);
        tim1.tv_nsec += nanoperiod;
        if(useconds>15) usleep(useconds-15); //use usleep for the bulk of the timing
        clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tim1, NULL);
    }
    return Py_None;
}

float readFew(int dev,int subdev,int chan,int range,int nreads)
{
    int maxdata,i;
    comedi_range *crange;
    float sum=0.0;
    float tmpval=0.0;
    comedi_t *it;
    if(dev==devno0) it=it0;
    else if(dev==devno1) it=it1;
    else it=it0;
    maxdata = comedi_get_maxdata(it,subdev,chan);
    crange = comedi_get_range(it,subdev,chan,range);
    if(nreads==1)
    {
        lsampl_t data;
        comedi_data_read(it,subdev,chan,range,aref, &data);
        sum = comedi_to_phys(data,crange,maxdata);
        if(sum!=sum&&data==0) sum = crange->min;
	else if(sum!=sum&&data==comedi_get_maxdata(it,subdev,chan)) sum = crange->max;
    }
    else
    {
        lsampl_t data[nreads];
        comedi_data_read_n(it,subdev,chan,range,aref, data, nreads);
        for(i=0;i<nreads;i++)
	{
	    tmpval=comedi_to_phys(data[i],crange,maxdata);
            if(tmpval!=tmpval&&data[i]==0) tmpval = crange->min;
	    else if(tmpval!=tmpval&&data[i]==comedi_get_maxdata(it,subdev,chan)) tmpval = crange->max;
	    sum+=tmpval;
	}
    }
    return sum/nreads;
}

float writeSteps(int dev,int subdev,int chan,int range,int start,int nsteps,int step,int uperiod)
{
    int i;
    int val;
    int res=0;
    struct timespec tim1;
    int nanoperiod = uperiod*1000;
    comedi_t *it;
    if(dev==devno0) it=it0;
    else if(dev==devno1) it=it1;
    else it=it0;
    clock_gettime(CLOCK_REALTIME, &tim1);
    for(i=0;i<nsteps;i++)
    {
	tim1.tv_nsec += nanoperiod;
        val = start+i*step;
        comedi_data_write(it,subdev,chan,range,aref,val);
        if(uperiod>50) usleep(uperiod-50);
	res = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tim1, NULL);
	if (res != 0){
                clock_gettime(CLOCK_REALTIME, &tim1);
		//printf("Failure");
	};
    }
    return start+i*step;
}


static PyObject *py_rwScan(PyObject *self, PyObject *args)
{
    int start,direction,navgs,step,nreads,nsteps,microsleep,uperiod,rmpstp;
    int i,j;
    int res=0;
    int tdiff=0;
    int rdev,rsubdev,rchan,rrange;
    int wdev,wsubdev,wchan,wrange;
    struct timespec tim1,tim2;

    if (!PyArg_ParseTuple(args, "(iiii)(iiii)iiiiiiiii",&rdev,&rsubdev,&rchan,&rrange,&wdev,&wsubdev,&wchan,&wrange,
        &start,&nsteps,&direction,&navgs,&step,&nreads,&rmpstp,&microsleep,&uperiod))
        return NULL;

    int length = (nsteps/navgs);
    PyObject *rlist= PyList_New(length);
    PyObject *wlist= PyList_New(length);
    PyObject *retlist = PyList_New(3);
    float sum=0.0;
    int current_dac = start;
    int maxdata;
    comedi_range *crange;
    comedi_t *it;
    if(wdev==devno0) it=it0;
    else if(wdev==devno1) it=it1;
    else it=it0;
    maxdata = comedi_get_maxdata(it,wsubdev,wchan);
    crange = comedi_get_range(it,wsubdev,wchan,wrange);
/*    int wstep = direction*step;*/
    for(i=0;i<length;i++)
    {
        PyList_SetItem(wlist, i, Py_BuildValue("f", comedi_to_phys(current_dac,crange,maxdata)));
        for(j=0;j<navgs;j++)
        {
            if(i==1) clock_gettime(CLOCK_REALTIME, &tim1);
            else tim1.tv_nsec += tdiff;
            usleep(microsleep);
            sum+=readFew(rdev,rsubdev,rchan,rrange,nreads);
            if (rmpstp>step||rmpstp<1) rmpstp=step;
            current_dac = writeSteps(wdev,wsubdev,wchan,wrange,current_dac,rmpstp,direction*(step/(rmpstp)),uperiod);
            if(i==1)
            {
                clock_gettime(CLOCK_REALTIME, &tim2);
                tdiff = tim2.tv_nsec - tim1.tv_nsec;
                tdiff *= 1.2;
                clock_gettime(CLOCK_REALTIME, &tim1);
            }
            else res = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tim1, NULL);
            if (res != 0){
                clock_gettime(CLOCK_REALTIME, &tim1);
                //printf("Failure!");
            };
        }
        PyList_SetItem(rlist, i, Py_BuildValue("f", sum/navgs));
        sum = 0.0;
    }
    res=PyList_SetItem(retlist, 0, rlist);
    if(res!=0) printf("Failed to set retlist item 0");
    res=PyList_SetItem(retlist, 1, wlist);
    if(res!=0) printf("Failed to set retlist item 1");
    res=PyList_SetItem(retlist, 2, Py_BuildValue("i",current_dac));
    if(res!=0) printf("Failed to set retlist item 2");
    return retlist;
}

static PyObject *py_readFew(PyObject *self, PyObject *args)
{
    int subdev,chan,range,dev;
    int nreads;
    float result=0.0;

    if (!PyArg_ParseTuple(args, "(iiii)i", &dev,&subdev,&chan,&range,&nreads))
        return NULL;

    result = readFew(dev,subdev,chan,range,nreads);
    return Py_BuildValue("f",result);
}

static PyObject *py_writeSteps(PyObject *self, PyObject *args)
{
    int subdev,chan,range,dev;
    int nsteps,step,start,uperiod;
//     float freq;
    int result=0;

    if (!PyArg_ParseTuple(args, "(iiii)iiii", &dev,&subdev,&chan,&range,&start,&nsteps,&step,&uperiod))
        return NULL;

    result = writeSteps(dev,subdev,chan,range,start,nsteps,step,uperiod);
    return Py_BuildValue("i",result);
}

static PyMethodDef comediExtensions_methods[] = {
    {"readFew", py_readFew, METH_VARARGS},
    {"writeSteps", py_writeSteps, METH_VARARGS},
    {"rwScan", py_rwScan, METH_VARARGS},
    {"initDevs", py_initDevs, METH_VARARGS},
    {"microsleep", py_microsleep, METH_VARARGS},
    {NULL, NULL}     /* Sentinel - marks the end of this structure */
};

PyDoc_STRVAR(initcomediExtensions__doc__,"Creates the task handle and loading handles for the functions.");

PyMODINIT_FUNC
initcomediExtensions(void)
{
    Py_InitModule3("comediExtensions", comediExtensions_methods,initcomediExtensions__doc__);
}
